/***************************************************************************
  * 
  * Homework for chapter 4 -- Adaptive filter using LMS & RLS
  *
  * Here is the realization of adapt_filtering function.
  * 
  **************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "adapt_filt.h"
#include <iostream>

using namespace std;

/**
 * @brief time-domain adaptive filter algorithm
 *
 * @param input          input audio sample of time index(n)
 * @param adapt_filter   adaptive filter buffer
 * @param filter_length  adaptive filter length, 128 by default
 * @param err            error output of time index(n)
 * @return 
 *     @retval 0         successfully
 */
int adapt_filtering_lms(short input, double* adapt_filter, int filter_length, short* err) {
    int i;
    double filter_output = 0.0;
    double error = 0.0;
    double mu = 0.2;

    // update inputdata buffer. Each sample is normalized to [-1,1)
    for(i = filter_length - 1; i > 0; i--) {
        inputdata[i] = inputdata[i - 1];
    }
    inputdata[0] = double(input / 32768.0);

    // finish adaptive filtering algorithm here, using LMS and RLS, respectively
    // 1. calculate filter output, as well as necessary parameters
//    double y_n = 0;
    for (int i = 0; i < filter_length; ++i) {
        filter_output += inputdata[i] * adapt_filter[i];
    }

    error = inputdata[0] - filter_output;
    double delta_n[filter_length];
    for (int i = 0; i < filter_length; ++i) {
        delta_n[i] = 2*error * inputdata[i];
    }

    // 2. update filter: adapt_filter, for future iterations

    for (int i = 0; i < filter_length; ++i) {
        adapt_filter[i] += mu*delta_n[i];
    }


    // end adaptive filtering algorithm

    // output error
    err[0] = short(error * 32768.0);

    return 0;
}

void init_rls(double theta){
    for(int i=0; i< filter_length; ++i){
        P[i][i]=theta;
    }
}

int adapt_filtering_rls(short input, double* adapt_filter, int filter_length, short* err) {
    int i;
    double filter_output = 0.0;
    double error = 0.0;
    double mu = 0.8;
    // update inputdata buffer. Each sample is normalized to [-1,1)
    for(i = filter_length - 1; i > 0; i--) {
        inputdata[i] = inputdata[i - 1];
    }
    inputdata[0] = double(input / 32768.0);

    // finish adaptive filtering algorithm here, using LMS and RLS, respectively
    // 1. calculate filter output, as well as necessary parameters

    for (int i = 0; i < filter_length; ++i) {
        filter_output += inputdata[i] * adapt_filter[i];
    }
    error = inputdata[0] - filter_output;


    double k[filter_length];

    for (int i = 0; i < filter_length; ++i) {
        k[i]=0;
        for (int j = 0; j < filter_length; ++j) {
            k[i] += P[i][j] * inputdata[j];
        }
    }

    double xTPx=0;
    for (int i = 0; i < filter_length; ++i) {

        xTPx += inputdata[i] * k[i];
    }


    for (int i = 0; i < filter_length; ++i) {
        k[i] /= mu + xTPx;
    }

    double xT_P[filter_length];
    for (int i = 0; i < filter_length; ++i) {
        xT_P[i] = 0;
        for (int j = 0; j < filter_length; ++j) {
            xT_P[i] += inputdata[j] * P[j][i];
        }
    }
    for (int i = 0; i < filter_length; ++i) {
        for (int j = 0; j < filter_length; ++j) {
            P[i][j] = (P[i][j] - k[i]*xT_P[j])/mu;
        }
    }



    // 2. update filter: adapt_filter, for future iterations

    for (int i = 0; i < filter_length; ++i) {
        adapt_filter[i] += error*k[i];
    }



    // end adaptive filtering algorithm

    // output error
    err[0] = short(error * 32768.0);

    return 0;

}

